// jjjjjj.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include<iostream>

#pragma comment(lib,"lib\\chai3d-debug.lib")

using namespace std;

//===========================================================================
/*
*/
//===========================================================================

//---------------------------------------------------------------------------
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#define GLUT_DISABLE_ATEXIT_HACK
#include <GL/glut.h>
//---------------------------------------------------------------------------
#include "chai3d.h"
//---------------------------------------------------------------------------


// initial size (width/height) in pixels of the display window
const int WINDOW_SIZE_W         = 800;
const int WINDOW_SIZE_H         = 600;

// mouse menu options (right button)
const int OPTION_FULLSCREEN     = 1;
const int OPTION_WINDOWDISPLAY  = 2;

// maximum number of haptic devices supported in this demo
const int MAX_DEVICES           = 8;


//---------------------------------------------------------------------------
// DECLARED VARIABLES
//---------------------------------------------------------------------------

// a world that contains all objects of the virtual environment
cWorld* world;

// a camera that renders the world in a window display
cCamera* camera;

// a light source to illuminate the objects in the virtual scene
cLight *light;

// width and height of the current window display
int displayW  = 0;
int displayH  = 0;

// a haptic device handler
cHapticDeviceHandler* handler;

// a pointer to the first haptic device detected on this computer
cGenericHapticDevice* hapticDevice = 0;

// a 3D cursor for the haptic device
cShapeSphere* cursor = 0;

// a line to display velocity of the haptic interface
cShapeLine* velocityVector;

// labels to show haptic device position and update rate
cLabel* positionLabel;
cLabel* rateLabel;
double rateEstimate = 0;

// material properties used to render the color of the cursors
cMaterial matCursorButtonON;
cMaterial matCursorButtonOFF;

// status of the main simulation haptics loop
bool simulationRunning = false;

// has exited haptics simulation thread
bool simulationFinished = false;

//---------------------------------------------------------------------------
// DECLARED FUNCTIONS
//---------------------------------------------------------------------------

// callback when the window display is resized
void resizeWindow(int w, int h);

// callback when a keyboard key is pressed
void keySelect(unsigned char key, int x, int y);

// callback when the right mouse button is pressed to select a menu item
void menuSelect(int value);

// function called before exiting the application
void close(void);

// main graphics callback
void updateGraphics(void);

// main haptics loop
void updateHaptics(void);


//===========================================================================
/*
    This application illustrates the use of the haptic device handler
    "cHapticDevicehandler" to access haptic devices connected to the computer.

    In this example the application opens an OpenGL window and displays a
    3D cursor for the first device found. If the operator presses the device
    user button, the color of the cursor changes accordingly.

    In the main haptics loop function  "updateHaptics()" , the position and 
    user switch status of the device are retrieved at each simulation iteration.
    This information is then used to update the position and color of the
    cursor. A force is then commanded to the haptic device to attract the 
    end-effector towards the device origin.
*/
//===========================================================================

int main(int argc, char* argv[])
{
    //-----------------------------------------------------------------------
    // INITIALIZATION
    //-----------------------------------------------------------------------

    cout<<"try to learn falcon"<<endl;
    //-----------------------------------------------------------------------
    // 3D - SCENEGRAPH
    //-----------------------------------------------------------------------

    // 创建一个场景
    world = new cWorld();

    // 将场景的背景色设置为（0,0,0）对应RGB为黑色
    world->setBackgroundColor(0, 0, 0);

    // 创建一个相机,构造相机的时候，要将其放在环境world下，并添加world的child为camera
    camera = new cCamera(world);
    //world->addChild(camera);也就是说上一句构造的时候已经调用了这个

    // 放置相机的位置
    camera->set( cVector3d (0.2, 0.0, 0.0),    // 相机放置位置（falcon 向外拉是x+，向里推是x-，上是z+，下是z-，左是y-，右是y+）
                 cVector3d (0.0, 0.0, 0.0),    // 看原点的位置
                 cVector3d (0.0, 0.0, 1.0));   // 相机的上方的方向

    // 设置摄像机看的范围从0.01 到 10
    camera->setClippingPlanes(0.01, 10.0);

    // 在场景中new一个灯
    light = new cLight(world);
    camera->addChild(light);                   // 将等添加到相机
    light->setEnabled(true);                   // 开启灯
    light->setPos(cVector3d( 2.0, 0.5, 1.0));  // 灯源所在位置
    light->setDir(cVector3d(-2.0, 0.5, 1.0));  // 灯找的方向

    // create a label that shows the haptic loop update rate
    rateLabel = new cLabel();
    rateLabel->setPos(8, 24, 0);
    camera->m_front_2Dscene.addChild(rateLabel);


    //-----------------------------------------------------------------------
    // HAPTIC DEVICES / TOOLS
    //-----------------------------------------------------------------------

    // create a haptic device handler
    handler = new cHapticDeviceHandler();

    // read the number of haptic devices currently connected to the computer
    int numHapticDevices = handler->getNumDevices();

    // if there is at least one haptic device detected...
    if (numHapticDevices)
    {
        // get a handle to the first haptic device
        handler->getDevice(hapticDevice);

        // open connection to haptic device
        hapticDevice->open();

		// initialize haptic device
		hapticDevice->initialize();

        // retrieve information about the current haptic device
        cHapticDeviceInfo info = hapticDevice->getSpecifications();

        // create a cursor with its radius set
        cursor = new cShapeSphere(0.01);

        // add cursor to the world
        world->addChild(cursor);

        // create a small line to illustrate velocity
        velocityVector = new cShapeLine(cVector3d(0,0,0), cVector3d(0,0,0));

        // add line to the world
        world->addChild(velocityVector);

        positionLabel = new cLabel();
        positionLabel->setPos(8, 8, 0);
        camera->m_front_2Dscene.addChild(positionLabel);
    }

    // here we define the material properties of the cursor when the
    // user button of the device end-effector is engaged (ON) or released (OFF)

    // a light orange material color
    matCursorButtonOFF.m_ambient.set(0.5, 0.2, 0.0);
    matCursorButtonOFF.m_diffuse.set(1.0, 0.5, 0.0);
    matCursorButtonOFF.m_specular.set(1.0, 1.0, 1.0);

    // a blue material color
    matCursorButtonON.m_ambient.set(0.1, 0.1, 0.4);
    matCursorButtonON.m_diffuse.set(0.3, 0.3, 0.8);
    matCursorButtonON.m_specular.set(1.0, 1.0, 1.0);


    //-----------------------------------------------------------------------
    // OPEN GL - WINDOW DISPLAY
    //-----------------------------------------------------------------------

    // initialize GLUT
    glutInit(&argc, argv);

    // retrieve the resolution of the computer display and estimate the position
    // of the GLUT window so that it is located at the center of the screen
    int screenW = glutGet(GLUT_SCREEN_WIDTH);
    int screenH = glutGet(GLUT_SCREEN_HEIGHT);
    int windowPosX = (screenW - WINDOW_SIZE_W) / 2;
    int windowPosY = (screenH - WINDOW_SIZE_H) / 2;

    // initialize the OpenGL GLUT window
    glutInitWindowPosition(windowPosX, windowPosY);
    glutInitWindowSize(WINDOW_SIZE_W, WINDOW_SIZE_H);
    glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
    glutCreateWindow(argv[0]);
    glutDisplayFunc(updateGraphics);
    glutKeyboardFunc(keySelect);
    glutReshapeFunc(resizeWindow);
    glutSetWindowTitle("CHAI 3D");

    // create a mouse menu (right button)
    glutCreateMenu(menuSelect);
    glutAddMenuEntry("full screen", OPTION_FULLSCREEN);
    glutAddMenuEntry("window display", OPTION_WINDOWDISPLAY);
    glutAttachMenu(GLUT_RIGHT_BUTTON);


    //-----------------------------------------------------------------------
    // START SIMULATION
    //-----------------------------------------------------------------------

    // simulation in now running
    simulationRunning = true;

    // create a thread which starts the main haptics rendering loop
    cThread* hapticsThread = new cThread();
    hapticsThread->set(updateHaptics, CHAI_THREAD_PRIORITY_HAPTICS);

    // start the main graphics rendering loop
    glutMainLoop();

    // close everything
    close();

    // exit
    return (0);
}

//---------------------------------------------------------------------------

void resizeWindow(int w, int h)
{
    // update the size of the viewport
    displayW = w;
    displayH = h;
    glViewport(0, 0, displayW, displayH);
}

//---------------------------------------------------------------------------

void keySelect(unsigned char key, int x, int y)
{
    // escape key
    if ((key == 27) || (key == 'x'))
    {
        // close everything
        close();

        // exit application
        exit(0);
    }
}

//---------------------------------------------------------------------------

void menuSelect(int value)
{
    switch (value)
    {
        // enable full screen display
        case OPTION_FULLSCREEN:
            glutFullScreen();
            break;

        // reshape window to original size
        case OPTION_WINDOWDISPLAY:
            glutReshapeWindow(WINDOW_SIZE_W, WINDOW_SIZE_H);
            break;
    }
}

//---------------------------------------------------------------------------

void close(void)
{
    // stop the simulation
    simulationRunning = false;

    // wait for graphics and haptics loops to terminate
    while (!simulationFinished) { cSleepMs(100); }

    // close the haptic devices
    if (hapticDevice)
    {
        hapticDevice->close();
    }
}

//---------------------------------------------------------------------------

void updateGraphics(void)
{
    // update the label showing the position of the haptic device
    if (cursor)
    {
        cVector3d position = cursor->getPos() * 1000.0; // convert to mm
        char buffer[128];
        sprintf(buffer, "device position: (%.2lf, %.2lf, %.2lf) mm",
            position.x, position.y, position.z);
        positionLabel->m_string = buffer;
    }

    // update the label with the haptic refresh rate
    char buffer[128];
    sprintf(buffer, "haptic rate: %.0lf Hz", rateEstimate);
    rateLabel->m_string = buffer;

    // render world
    camera->renderView(displayW, displayH);

    // Swap buffers
    glutSwapBuffers();

    // check for any OpenGL errors
    GLenum err;
    err = glGetError();
    if (err != GL_NO_ERROR) printf("Error:  %s\n", gluErrorString(err));

    // inform the GLUT window to call updateGraphics again (next frame)
    if (simulationRunning)
    {
        glutPostRedisplay();
    }
}

//---------------------------------------------------------------------------

void updateHaptics(void)
{
    // a clock to estimate the haptic simulation loop update rate
    cPrecisionClock pclock;
    pclock.setTimeoutPeriodSeconds(1.0);
    pclock.start(true);
    int counter = 0;

    // main haptic simulation loop
    while(simulationRunning)
    {
		if (!hapticDevice) continue;
		
        // read position of haptic device
        cVector3d newPosition;
        hapticDevice->getPosition(newPosition);

        // update position and orientation of cursor
        cursor->setPos(newPosition);

        // read linear velocity from device
        cVector3d linearVelocity;
        hapticDevice->getLinearVelocity(linearVelocity);

        // update the line showing velocity
        velocityVector->m_pointA = newPosition;
        velocityVector->m_pointB = newPosition + linearVelocity;

        // read user button status
        bool buttonStatus;
        hapticDevice->getUserSwitch(0, buttonStatus);

        // adjust the color of the cursor according to the status of
        // the user switch (ON = TRUE / OFF = FALSE)
        cursor->m_material = buttonStatus ? matCursorButtonON : matCursorButtonOFF;

        // compute a reaction force (a spring that pulls the device to the origin)
        const double stiffness = 100.0; // [N/m]
        //cVector3d force = -stiffness * newPosition;
		cVector3d force (0,0,2);

        // send computed force to haptic device
        hapticDevice->setForce(force);

        // estimate the refresh rate
        ++counter;
        if (pclock.timeoutOccurred()) {
            pclock.stop();
            rateEstimate = counter;
            counter = 0;
            pclock.start(true);
        }
    }
    
    // exit haptics thread
    simulationFinished = true;
}

//---------------------------------------------------------------------------

